#include "Shooter.h"
#include "../Robotmap.h"
#include "../CommandBase.h"
#include "../NetworkMap.h"
#define ShooterJoy CommandBase::oi->shooterJoystick
Shooter::Shooter() :
	Subsystem("Shooter"), flipper(SOLENOID_MODULE, SHOOTER_SOLENOID),/*flipperBack(SHOOTER_SOLENOID_BACK),*/
	shooterVictor(SHOOTER_VICTOR) {
	flipperExtendN = new Notifier(TimerEventHandler(Shooter::ExtendFlipper),
			(void*) this);
	flipperRetractN = new Notifier(TimerEventHandler(Shooter::RetractFlipper),
			(void*) this);
	flipperPulseN = new Notifier(TimerEventHandler(Shooter::StaticPulse), (void*) this);
	flipper.Set(false);
	isOut = false;
}

void Shooter::InitDefaultCommand() {
	// Set the default command for a subsystem here.
	//SetDefaultCommand(new MySpecialCommand());
}
void Shooter::Pulse() {
	flipperExtendN->StartSingle(0.0);
	flipperRetractN->StartSingle(0.20);
}

void Shooter::StaticPulse(void * handle)
{
	Shooter* shooter = (Shooter*) handle;
	shooter->Pulse();
}

void Shooter::ExtendFlipper(void* shooterP)
{
	Shooter* sH = (Shooter*) shooterP;
	sH->SetFlipperMode(kOn);
	sH->isOut = true;
}

void Shooter::RetractFlipper(void* shooterP)
{
	Shooter* sH = (Shooter*) shooterP;
	sH->SetFlipperMode(kOff);
	sH->isOut = false;
}

void Shooter::PeriodicUpdate()
{

}

void Shooter::SetFlipperMode(FlipperMode Mode) {
	if (Mode == kOn) {
		flipper.Set(true);
	} else {
		flipper.Set(false);
	}
	CommandBase::client->SetNWInt(flprSol, (int)flipper.Get());
}

void Shooter::SetShooterMode(ShooterMode Mode) {
	if (Mode == Shooter::kFire) {
		shooterVictor.SetRaw(255);
	} else {
		shooterVictor.SetRaw(255);
	}
	CommandBase::client->SetNWFloat(shtVic, shooterVictor.Get());
}
